#include <chrono>
#include <memory>
#include <rclcpp/rclcpp.hpp>
#include "std_msgs/msg/int32.hpp"
#include <thread>

bool ctrl_flag = false;

static void my_handler(int sig) // can be called asynchronously
{
  ctrl_flag = true; // set flag
}

using namespace std::chrono_literals;

class MultiThreadedPublisher : public rclcpp::Node
{
public:
  MultiThreadedPublisher() : Node("multi_threaded_publisher")
  {

    // // 实例化回调组
    callback_group = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
    callback_group_2 = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);

    // callback_group = nullptr;

    timer1_ = this->create_wall_timer(std::chrono::milliseconds(5000), std::bind(&MultiThreadedPublisher::publish_message1, this), callback_group);
    timer2_ = this->create_wall_timer(std::chrono::milliseconds(1000), std::bind(&MultiThreadedPublisher::publish_message2, this), callback_group_2);
  }

private:
  void publish_message1()
  {
    while (1)
    {
      if (ctrl_flag == true)
      {
        rclcpp::shutdown();
        exit(0);
      }
      usleep(10000);
      RCLCPP_INFO(this->get_logger(), "Publishing on topic1: '%d'", 1);
    }
  }

  void publish_message2()
  {
    RCLCPP_INFO(this->get_logger(), "Publishing on topic2: '%d'", 2);
  }

  rclcpp::TimerBase::SharedPtr timer1_;
  rclcpp::TimerBase::SharedPtr timer2_;
  // 声明回调组 必须放在这里！！
  rclcpp::CallbackGroup::SharedPtr callback_group;
  rclcpp::CallbackGroup::SharedPtr callback_group_2;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  signal(SIGINT, my_handler); 
  rclcpp::executors::MultiThreadedExecutor executor(rclcpp::ExecutorOptions(), 20, true);
  auto node = std::make_shared<MultiThreadedPublisher>();
  executor.add_node(node);
  executor.spin();
  rclcpp::shutdown();
  return 0;
  return EXIT_SUCCESS;
}
